from numpy import *
import csv

delta_t = 0.005

# K = array([1])
# P = array([1])
# PP = array([1])
#
# A = array([1])
# H = array([1])
# F = array([delta_t])

Cov1, Cov2 = [], []

fileCov = open('Covariance.csv', 'a')
Cov_writer = csv.writer(fileCov)
Cov_writer.writerow(['Cov_PP', 'Cov_P'])

def axisFilterBias(acce_state, acce_measure, F_acce, Q_acce, H_acce, R_acce, P_acce, K_acce):

    z = acce_measure
    x = acce_state

    x = dot(F_acce, x)
    PP_acce = dot(dot(F_acce, P_acce), F_acce.T) + Q_acce

    K_acce = dot(PP_acce, H_acce.T) / (dot(dot(H_acce, PP_acce), H_acce.T) + R_acce)

    y_k = z - dot(H_acce, x)
    x = x + dot(K_acce, y_k)
    P_acce = PP_acce - dot(dot(K_acce, H_acce), PP_acce)

    return [x, z, F_acce, Q_acce, H_acce, R_acce, P_acce, K_acce]




def axisFilter(a_state, a_measure, k, p, pp, a, h, f, q, r):

    z = a_measure
    x = a_state

    ## gengxinbu zai zheli gai X de zhi
    ### bug: shunxu diandao yixia
    # x = dot(a, x) + dot(k, (z - dot(h, dot(a, x))))
    x = dot(a, z)

    pp_last = pp
    #### bug: P and PP, hunyong le, zheli cankaode gongshi yinggai xiecuo le, woshi duide
    pp = dot(a, dot(p, a.T)) + dot(f, dot(q, f.T))

    k = dot(pp, dot(h.T, linalg.inv(dot(h, dot(pp, h.T)) + r)))

    ### huanle yige xinde guance bu fangcheng weizhi
    # x = dot(a, x) + dot(k, (z - dot(h, dot(a, x))))

    p = pp - dot(k, dot(h, pp))
    ### zheli de p gongshi xiecuo le

    # p = pp_last - dot(k, dot(h, p))

    Cov1.append(pp[0][0])
    Cov2.append(p[0][0])

    Cov_writer.writerow([pp[0][0], p[0][0]])

    return [x, z, k, p, pp, a, h, f, q, r]



# delta_t = 0.005
# X = array([wp, wq, wr, 0, 0, 0])
# Z = array([0, 0, 0])
#
# k = array([[1, 0, 0],
#            [0, 1, 0],
#            [0, 0, 1],
#            [0, 0, 0],
#            [0, 0, 0],
#            [0, 0, 0]])
# P = array([[1, 0, 0, 0, 0, 0],
#            [0, 1, 0, 0, 0, 0],
#            [0, 0, 1, 0, 0, 0],
#            [0, 0, 0, 1, 0, 0],
#            [0, 0, 0, 0, 1, 0],
#            [0, 0, 0, 0, 0, 1]])
# PP = array([[1, 0, 0, 0, 0, 0],
#            [0, 1, 0, 0, 0, 0],
#            [0, 0, 1, 0, 0, 0],
#            [0, 0, 0, 1, 0, 0],
#            [0, 0, 0, 0, 1, 0],
#            [0, 0, 0, 0, 0, 1]])
#
# A = array([[1, 0, 0, delta_t, 0, 0],
#            [0, 1, 0, 0, delta_t, 0],
#            [0, 0, 1, 0, 0, delta_t],
#            [0, 0, 0, 1, 0, 0],
#            [0, 0, 0, 0, 1, 0],
#            [0, 0, 0, 0, 0, 1]])
# H = array([[1, 0, 0, 0, 0, 0],
#            [0, 1, 0, 0, 0, 0],
#            [0, 0, 1, 0, 0, 0]])
# F = array([[0.5 * delta_t ^ 2, 0, 0],
#            [0, 0.5 * delta_t ^ 2, 0],
#            [0, 0, 0.5 * delta_t ^ 2],
#            [delta_t, 0, 0],
#            [0, delta_t, 0],
#            [0, 0, delta_t]])
#
# Q = 100 * np.identity(3)
# R = 0.01 * np.identity(3)
#
# Z = array([wp, wq, wr])
#
# X = A * X + K * (Z - H * (A * X))
#
# K = X * H.T * linalg.inv(H * PP * H.T + R)
# PP = A * P * A.T + F * Q * F.T
# P = PP - K * H * PP
#
# wp = X[0]
# wq = X[1]
# wr = X[2]
# dp = X[3]
# dq = X[4]
# dr = X[5]
